# Simple little python script to facilitate interfacing with team SpotOn's robot motor
# controller.  This program requires the pyserial library found at http://pyserial.sourceforge.net
#
# Author:    Clark Scheff
# Date:        05/11/2008


import serial     # import serial communications module
import sys        # import system module
from time import sleep

startbyte = '!'
commands = ['F','R','TL','TR','RL','RR','IL','DL','IR','DR','RLT','RRT','TLTR','TRTR','BT','B','TLT','TRT','FT','GT','GTPS','GS','ID','DD','?','QUIT']


# prompt for serial port to open
port = raw_input("Port to open: ").strip()
baud = raw_input("Baud rate: ").strip()

try:
    serlink = serial.Serial(port, int(baud))
except serial.SerialException:
    print "Error opening serial port!"
    print "Exiting..."
    sys.exit(69)
    
print "Opening (" + serlink.portstr + ", " + baud + ", 8, N, 1).\r\n"

running = True
while running == True:
    cmd = raw_input("Enter a command (? for help): ").strip()
    if cmd == '?':
        print "Available commands:\r\n"
        for i in commands:
            print i + " "

    if cmd == 'QUIT':
        running = False

    found = False
    for i in commands:
        if cmd == i:
            found = True
    
    if found == False:
        print "Invalid command.  Enter ? for a list of available commands."
    else:
        if commands.index(cmd) <= 5:
            speed = 0
	    try:
		speed = int(raw_input("Enter a speed for this command (0-127): ").strip())
            	if speed < 0 or speed > 127:
                	print "Invalid speed value, did I mention it must be between 0 and 127?  Oh yeah, I did!"
            	else:
                	serlink.write(startbyte + chr(commands.index(cmd)+65) + chr(speed))
                	print "Sending ["+cmd+", "+str(speed)+"]"
	    except ValueError:
		print "The value entered is not a number, please try again."

        elif (commands.index(cmd) >= 16 and commands.index(cmd) <= 18) or (commands.index(cmd) >= 10 and commands.index(cmd) <= 13) or commands.index(cmd) == 14:
            tick = int(raw_input("Enter a TICK value for this command (0-255): ").strip())
            if tick < 0 or tick > 255:
                print "Invalid TICK value, did I mention it must be between 0 and 255?  Oh yeah, I did!"
            else:
                serlink.write(startbyte + chr(commands.index(cmd)+65) + chr(tick))
                print "Sending ["+cmd+", "+str(tick)+"]"

	elif cmd == 'GT' or cmd == 'GTPS':
	    serlink.flushInput();
	    serlink.write(startbyte + chr(commands.index(cmd)+65) + chr(0))
	    sleep(0.1)
	    tick_report = serlink.read(serlink.inWaiting());
	    print "Received: " + tick_report;

        elif cmd != 'QUIT':
            serlink.write(startbyte + chr(commands.index(cmd)+65) + chr(0))
            print "Sending ["+cmd+"]"


print "Sending BRAKE command, just to be safe!"
serlink.write('!P ')
print "Closing serial port..."
serlink.close()
print "Hasta luego!"
